
/*
 * Copyright 1996-2020 Cyberbotics Ltd.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*
 * Description:  A controller moving the Pioneer3DX and its gripper.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <webots/differential_wheels.h>
#include <webots/distance_sensor.h>
#include <webots/motor.h>
#include <webots/robot.h>
#include <webots/compass.h>
#include <webots/gps.h>
#include <webots/device.h>

#include <webots/keyboard.h>
#include <webots/camera.h>
#include <webots/display.h>


#define GRIPPER_MOTOR_MAX_SPEED 0.1
#define TIME_STEP 32
//内环INNER边长0.73 外环OUTER边长1.2
#define INNER 0.84
#define OUTER 1.2

static WbDeviceTag wheel_motors[3];
static WbDeviceTag gripper_motors[3];
//static WbDeviceTag lift_position;
static int time_step = 0;

static WbDeviceTag camera;
static WbDeviceTag compass;
static WbDeviceTag gps;
WbDeviceTag gps_display;

static float moveSpd = 1.0;

typedef struct Point
{
  double x;
  double y;
}Point;


static void initialize() {
  /* necessary to initialize Webots */
  wb_robot_init();

  time_step = wb_robot_get_basic_time_step();
  
  int n_devices = wb_robot_get_number_of_devices();
  int i;
  for(i=0; i<n_devices; i++) {
    WbDeviceTag tag = wb_robot_get_device_by_index(i);
  
    const char *name = wb_device_get_name(tag);
    WbNodeType type = wb_device_get_node_type(tag);
  
    // do something with the device
    printf("Device #%d name = %s\n", i, name);
  
    if (type == WB_NODE_CAMERA) {
      // do something with the camera
      printf("Device #%d is a camera\n", i);
    }
  }

  gripper_motors[0] = wb_robot_get_device("lift motor");
  gripper_motors[1] = wb_robot_get_device("left finger motor");
  gripper_motors[2] = wb_robot_get_device("right finger motor");
  wheel_motors[0] = wb_robot_get_device("left wheel");
  wheel_motors[1] = wb_robot_get_device("right wheel");
  //lift_position = wb_robot_get_device("lifr motor position sensor");
  // Specify velocity control mode
  wb_motor_set_position(wheel_motors[0], INFINITY);
  wb_motor_set_position(wheel_motors[1], INFINITY);
  wb_motor_set_velocity(wheel_motors[0], 0.0);
  wb_motor_set_velocity(wheel_motors[1], 0.0);
  //wb_position_sensor_enable(lift_position,TIME_STEP);
  
  camera = wb_robot_get_device("camera");
  wb_camera_enable(camera,TIME_STEP);
  wb_camera_recognition_enable(camera,TIME_STEP);
  
  compass = wb_robot_get_device("compass");
  wb_compass_enable(compass,TIME_STEP);
  
  gps = wb_robot_get_device("gps");
  wb_gps_enable(gps,TIME_STEP);
  
  gps_display = wb_robot_get_device("gps_display");
  wb_display_set_color(gps_display,0);
  wb_display_set_font(gps_display, "Arial", 16, true);
}

void step(double seconds) {
  const double ms = seconds * 1000.0;
  int elapsed_time = 0;
  while (elapsed_time < ms) {
    wb_robot_step(time_step);
    elapsed_time += time_step;
  }
}

void lift(double position) {
  wb_motor_set_velocity(gripper_motors[0], GRIPPER_MOTOR_MAX_SPEED);
  wb_motor_set_position(gripper_motors[0], position);
}

void moveFingers(double position) {
  wb_motor_set_velocity(gripper_motors[1], GRIPPER_MOTOR_MAX_SPEED);
  wb_motor_set_velocity(gripper_motors[2], GRIPPER_MOTOR_MAX_SPEED);
  wb_motor_set_position(gripper_motors[1], position);
  wb_motor_set_position(gripper_motors[2], position);
}

void moveForwards(double speed) {
  wb_motor_set_velocity(wheel_motors[0], speed);
  wb_motor_set_velocity(wheel_motors[1], speed);
}

void turn(double speed) {
  wb_motor_set_velocity(wheel_motors[0], speed);
  wb_motor_set_velocity(wheel_motors[1], -speed);
}

void stop(double seconds) {
  wb_motor_set_velocity(wheel_motors[0], 0.0);
  wb_motor_set_velocity(wheel_motors[1], 0.0);
  step(seconds);
}

void disPlay(WbDeviceTag tag,const char* str,int px,int py,int display_width,int display_height)
{
  wb_display_set_alpha(tag, 0.0);
  wb_display_fill_rectangle(tag, 0, 0, display_width, display_height);
  wb_display_set_alpha(tag, 1.0);
  wb_display_draw_text(tag, str,0,0);
}

double calAngle()
{
  step(0.001);
  const double* cmps_raw = wb_compass_get_values(compass);
  double angle = atan2(cmps_raw[0],cmps_raw[2]);
  angle = (angle-3.1416) / M_PI * 180;//convert to degree
  if(angle < 0.0)
    angle = angle + 360.0;
  return angle;
}

void showData()
{
  const double* gps_raw;
  char* gps_position = (char*)malloc(100);
  int display_width = wb_display_get_width(gps_display);
  int display_height = wb_display_get_height(gps_display);
  
  step(0.001);
  gps_raw = wb_gps_get_values(gps);
  sprintf(gps_position,"gps=(%.3f,%.3f)\nangle = %.3f",gps_raw[0],gps_raw[2],calAngle());
  disPlay(gps_display,gps_position,0,0,display_width,display_height);
}

void adjustAng(double angle)
{//0--360
  double ang = calAngle();
  while(abs(ang - angle) > 0.5)
  {
    //printf("%f\n",angle);
    showData();
    if(angle > 180)
    {
      if(ang > angle - 180 && ang < angle)
        turn(1.0);
      else
        turn(-1.0);
    }
    else
    {
      if(ang > angle && ang < angle+180)
        turn(-1.0);
      else
        turn(1.0);
    }
    ang = calAngle();
  }
  stop(0.0);
}

double calDistance(Point p1,Point p2)
{
   double dx = p1.x - p2.x;
   double dy = p1.y - p2.y;
   return sqrt(dx*dx+dy*dy);
}

float max(float x1,float x2)
{
  if(x1 > x2)
    return x1;
  else
    return x2;
}
float min(float x1,float x2)
{
  if(x1 > x2)
    return x2;
  else
    return x1;
}
Point getCurPos()
{
  step(0.001);
  const double* gps_raw = wb_gps_get_values(gps);
  Point pos = {gps_raw[0],gps_raw[2]};
  return pos;
}

void movep2p(Point dst)
{
  //仓库区域大约是|x|<0.73 & |y|<0.73
  if(fabs(dst.x) < INNER && fabs(dst.y) < INNER)
  {
    //printf("(%f,%f)\n",fabs(dst.x),fabs(dst.y));
    printf("cannot reach\n");
    return;
  }
  step(0.001);
  //get current position
  const double* gps_raw = wb_gps_get_values(gps);
  Point pos = {gps_raw[0],gps_raw[2]};
  //choose path
  while(fabs(pos.x - dst.x) > 0.2 || fabs(pos.y - dst.y) > 0.2)
  {//还没到达目的地
    showData();
    step(0.001);
    gps_raw = wb_gps_get_values(gps);
    pos.x = gps_raw[0];
    pos.y = gps_raw[2];
    if(calDistance(pos,dst) < 0.1)
    {
      stop(0.0);
      break;
    }
    if(fabs(pos.y) > INNER)
    {//x轴方向可以走
      if(pos.x < dst.x)
      {
        adjustAng(0.0);//x整方向
        moveForwards(1.5); 
        while(gps_raw[0] < max(INNER,dst.x))
        {
          step(0.001);
          showData();
          gps_raw = wb_gps_get_values(gps);
          pos.x = gps_raw[0];
          pos.y = gps_raw[2];
          if(calDistance(pos,dst) < 0.1)
            break;
        }
        stop(0.0);
      }
      else
      {
        adjustAng(180.0);//x负方向
        moveForwards(1.5);
        while(gps_raw[0] > min(dst.x,INNER))
        {
          step(0.001);
          showData();
          gps_raw = wb_gps_get_values(gps);
          pos.x = gps_raw[0];
          pos.y = gps_raw[2];
          if(calDistance(pos,dst) < 0.1)
            break;
        }
        stop(0.0);
      }
    }
    if(fabs(pos.x) > INNER)
    {
      if(pos.y < dst.y)
      {
        adjustAng(90);
        moveForwards(1.5);
        while(gps_raw[2] < max(dst.y,INNER))
        {
          step(0.001);
          showData();
          gps_raw = wb_gps_get_values(gps);
          pos.x = gps_raw[0];
          pos.y = gps_raw[2];
          if(calDistance(pos,dst) < 0.1)
            break;
        }
        stop(0.0);
      }
      else
      {
        adjustAng(270);
        moveForwards(1.5);
        while(gps_raw[2] > min(dst.y,INNER))
        {
          step(0.001);
          showData();
          gps_raw = wb_gps_get_values(gps);
          pos.x = gps_raw[0];
          pos.y = gps_raw[2];
          if(calDistance(pos,dst) < 0.1)
            break;
        }
        stop(0.0);
      }
    }
  }
  printf("movep2p finished\n");
}

float LineValue(Point p1,Point p2,Point input)
{
  //直线方程一般式计算值
  return (p2.y-p1.y)*input.x + (p1.x-p2.x)*input.y + p2.x*p1.y - p1.x*p2.y;
}

Point throughcorner(Point pos,Point dst,Point* corner)
{//判断是否需要经过角点
  float val[4];
  Point bestcorner = {0,0};
  int i;
  for(i = 0;i < 4;i++)
  {
    val[i] = LineValue(pos,dst,corner[i]);
  }
  if((val[0] * val[3] < 0) || (val[1] * val[2] < 0))
  {
    //找最近的两个角
    float dis[4];
    for(i = 0;i < 4;i++)
    {
      dis[i] = calDistance(pos,corner[i]);
    }
    int k = 0;
    float distmp = dis[0];
    Point cor1 = corner[0];
    Point cor2 = corner[0];
    for(i = 1;i < 4;i++)
    {
      if(distmp > dis[i])
      {
        distmp = dis[i];
        cor1 = corner[i];
        k = i;
      }
    }
    dis[k] = 99;
    distmp = 99;
    for(i = 0;i<4;i++)
    {
      if(distmp > dis[i])
      {
        distmp = dis[i];
        cor2 = corner[i];
      }
    }
    //根据局部最优两个角点择优
    if(calDistance(cor1,dst) < calDistance(cor2,dst))
    {
      if(cor1.x>0)
        bestcorner.x = cor1.x+0.3;//膨胀操作，防止撞
      else
        bestcorner.x = cor1.x-0.3;
      if(cor1.y > 0)
        bestcorner.y = cor1.y+0.3;
      else
        bestcorner.y = cor1.y-0.3;
    }
    else
    {
      if(cor2.x > 0)
        bestcorner.x = cor2.x+0.3;//膨胀操作，防止撞
      else
        bestcorner.x = cor2.x-0.3;
      if(cor2.y > 0)
        bestcorner.y = cor2.y+0.3;
      else
        bestcorner.y = cor2.y-0.3;
    }
  }
  return bestcorner;
}

void new_movep2p(Point dst)
{
  //define corner
  float dilate = 0.1;//膨胀，防止碰撞
  Point corner[4] = {{0.6+dilate,0.6+dilate},
                  {0.6+dilate,-0.6-dilate},
                  {-0.6-dilate,0.6+dilate},
                  {-0.6-dilate,-0.6-dilate}};
  
  //导航
  Point pos = getCurPos();
  while(calDistance(pos,dst) > 0.1)
  {
    //判断是否要经过转折点
    Point tmp = throughcorner(pos,dst,corner);
    if(tmp.x == 0)
    {
      //直接抵达目的地
      printf("直接抵达\n");
      double p2pAng = atan2(dst.y-pos.y,dst.x-pos.x);
      p2pAng = p2pAng/M_PI*180;
      if(p2pAng < 0)
        p2pAng += 360;
      adjustAng(p2pAng);
      moveForwards(moveSpd);
      while(calDistance(pos,dst) > 0.1)
      {
        pos = getCurPos();
        showData();
      }
    }
    else
    {
      //经过角点
      printf("经过角点\n");
      double p2pAng = atan2(tmp.y-pos.y,tmp.x-pos.x);
      p2pAng = p2pAng/M_PI*180;
      if(p2pAng < 0)
        p2pAng += 360;
      adjustAng(p2pAng);
      moveForwards(moveSpd);
      while(calDistance(pos,tmp) > 0.1)
      {
        pos = getCurPos();
        showData();
      }
    }
    pos = getCurPos();
  }
  stop(0.0);
  printf("new_p2p finished\n");
}

void adjustPos(Point dst,float angle)
{
  Point pos = getCurPos();
  if(calDistance(pos,dst) > 0.05)
  {
    double p2pAng = atan2(dst.y-pos.y,dst.x-pos.x);
    p2pAng = p2pAng/M_PI*180;
    if(p2pAng < 0)
      p2pAng += 360;
    printf("%f\n",p2pAng);
    adjustAng(p2pAng);
    moveForwards(0.5);
    while(calDistance(pos,dst) > 0.01)
    {
      pos = getCurPos();
      showData();
    }
  }
  adjustAng(angle);
}
char emptyPlace()
{
  char empty = 0x00;
  /*
  0001左下
  0010右下
  0100右上
  1000左上
  */
  int obnum = 0;
  const WbCameraRecognitionObject* objects;
  
  objects = wb_camera_recognition_get_objects(camera);
  obnum = wb_camera_recognition_get_number_of_objects(camera);
  
  for(int i=0;i<obnum;i++)
  {
    printf("id=%d,(%.3f,%.3f,%.3f)\n",objects[i].id,objects[i].position[0],objects[i].position[1],objects[i].position[2]);
  }
  if(obnum != 0)
  {
    for(int i=0; i<obnum;i++)
    {
      if(objects[i].position[0]<0 && objects[i].position[1] < 0)
        empty |= 0x01;
      if(objects[i].position[0]>0 && objects[i].position[1] < 0)
        empty |= 0x02;
      if(objects[i].position[0]>0 && objects[i].position[1] > 0)
        empty |= 0x04;
      if(objects[i].position[0]<0 && objects[i].position[1] > 0)
        empty |= 0x08;
    }
  }
  if(empty&0x01)
    printf("1\n");
  if(empty&0x02)
    printf("2\n");
  if(empty&0x04)
    printf("3\n");
  if(empty&0x08)
    printf("4\n");
  if(!(empty&0x0f))
    printf("empty\n");
  return empty;
}

void putDown_approach(int shelf,int index)
{
  moveForwards(1.0);
  Point pos = getCurPos();
  double targetPos = 0.05;
  switch(shelf)
  {//走到近前
    case 1:
      if(index == 3 || index == 4)
        targetPos = -0.15;
      else
        targetPos = 0.0;
      lift(targetPos);
      while(pos.x > -1.46)
        pos = getCurPos();
      break;
    case 2:
      if(index == 3 || index == 4)
        targetPos = -0.15;
      else
        targetPos = 0.0;
      lift(targetPos);
      while(pos.x < 1.46)
        pos = getCurPos();
      break;
    case 3:
      if(index == 3 || index == 4)
        targetPos = -0.35;
      else
        targetPos = 0.0;
      lift(targetPos);
      while(pos.y > -1.46)
        pos = getCurPos();
      break;
    case 4:
      if(index == 3 || index == 4)
        targetPos = -0.35;
      else
        targetPos = 0.0;
      lift(targetPos);
      while(pos.y < 1.46)
        pos = getCurPos();
      break;
    default:
      break;
  }
  stop(0.0);
}
void putDown_away(int shelf)
{
  moveForwards(-1.0);
  Point pos = getCurPos();
  switch(shelf)
  {//走到近前
    case 1:
      while(pos.x < -1)
        pos = getCurPos();
      break;
    case 2:
      while(pos.x > 1)
        pos = getCurPos();
      break;
    case 3:
      while(pos.y < -1)
        pos = getCurPos();
      break;
    case 4:
      while(pos.y > 1)
        pos = getCurPos();
      break;
    default:
      break;
  }
  stop(0.0);
}

void putDown(int index,int shelf)
{//index为货架编号
  double ang = calAngle();
  if(index == 1 || index == 4)
    ang -= 10;
  else 
    ang += 10;

  adjustAng(ang);
  //抬起爪子并靠近
  putDown_approach(shelf,index);
  printf("放货中……\n");
  moveFingers(0.07);
  step(1.5);
  //离开
  putDown_away(shelf);
}

void putGoods(int shelf)
{
  moveSpd = 2.0;//设置移动速度
  Point shelfPos;
  double shelfAng = 0;
  switch(shelf)
  {
    case 1:
      printf("1号柜\n");
      shelfPos.x = -0.92;
      shelfPos.y = 0.76;
      shelfAng = 180;
      break;
    
    case 2:
      printf("2号柜\n");
      shelfPos.x = 0.92;
      shelfPos.y = -0.98;
      shelfAng = 0;
      break;
  
    case 3:
      printf("3号柜\n");
      shelfPos.x = -0.77;
      shelfPos.y = -0.95;
      shelfAng = 270;
      break;
      
    case 4:
      printf("4号柜\n");
      shelfPos.x = 0.78;
      shelfPos.y = 0.95;
      shelfAng = 90;
      break;
      
    default:
      printf("goods shelf index error\n");
  }
  
  while(true)
  {
    //走到货架前，并调整姿态
    new_movep2p(shelfPos);
    adjustPos(shelfPos,shelfAng);
    //选择空货架
    char empty = emptyPlace();
    if(!(empty&0x01))
    {
      putDown(1,shelf);
      break;
    }
    else if(!(empty&0x02))
    {
      putDown(2,shelf);
      break;
    }
    else if(!(empty&0x04))
    {
      putDown(3,shelf);
      break;
    }
    else if(!(empty&0x08))
    {
      putDown(4,shelf);
      break;
    }
    else
    {
      printf("全满了，调整位置\n");
      //当前位置全满，换位置
      switch(shelf)
      {
        case 1:
          shelfPos.y -= 0.5;
          break;
        case 2:
          shelfPos.y += 0.5;
          break;
        case 3:
          shelfPos.x += 0.5;
          break;
        case 4:
          shelfPos.x -= 0.5;
          break;
        default:
          printf("goods shelf index error\n");
      }
    }
  }

    
}

void searchGoods()
{
  moveSpd = 3.0;//设置移动速度，比较快
  Point sgP[8] = {{-1.0,0.3},
                {-1.0,-0.3},
                {-0.3,-1.0},
                {0.3,-1},
                {1.0,-0.3},
                {1.0,0.3},
                {0.3,1.0},
                {-0.3,1.0}};
  float sgA[8] = {0,0,90,90,180,180,270,270};
  //依次寻找1、2、3、4
  int i,j;
  int obnum = 0;
  bool hasGoods = false;
  const WbCameraRecognitionObject* objects;
  for(i = 0;i < 8;i++)
  {
    new_movep2p(sgP[i]);
    adjustPos(sgP[i],sgA[i]);
    //adjustAng(sgA[i]);
    printf("到达%d号取货点\n",i);
    stop(0.0);
    //*********可能需要左转右转调整一下视角***********/
    obnum = wb_camera_recognition_get_number_of_objects(camera);
    objects = wb_camera_recognition_get_objects(camera);
    if(obnum > 0)
    {
      for(j = 0;j<obnum;j++)
        if(objects[j].position[2] > -1.5)
          hasGoods = true;
    }
    if(hasGoods)
      break;
    printf("没有货，下一个取货点\n");
  }
  /*****************取货************************************/
  if(hasGoods)
  {
    printf("取%d号取货点的货物……\n",i);
    /***************get goods***********************/
    
    
    step(5.0);
    
  }
  
}

int main() {
  initialize();

  double liftposition = 0.05;
  double fingerposition = 0.06;
  lift(liftposition);
  step(2.0);
  
  int pc = 0;
  wb_keyboard_enable(TIME_STEP);
  
  char empty;
  
  //step1:寻找货物，并抓取
  searchGoods();
  //step2:放货
  //int shelf = 4;//放到几号货柜，货架编号1-4
  //putGoods(shelf);

  while(true)
  {
    step(0.01);
    showData();

    int c = wb_keyboard_get_key();
    if((c>=0) && c!= pc)
    {
      switch(c)
      {
        case 'W':
          moveForwards(2.5);
          //adjustAng(0);
          break;
        case 'S':
          moveForwards(-2.5);
          //adjustAng(90);
          break;
        case 'A':
          turn(-1.5);
          //adjustAng(180);
          break;
        case 'D':
          turn(1.5);
          //adjustAng(270);
          break;
        case 'P':
          stop(0.0);
          break;
        case 'I':
          liftposition -= 0.05;
          lift(liftposition);
          break;
        case 'K':
          liftposition += 0.05;
          lift(liftposition);
          break;
        case 'J':
          fingerposition+=0.01;
          moveFingers(fingerposition);
          break;
        case 'L':
          fingerposition-=0.01;
          moveFingers(fingerposition);
          break;
        case 'R':
          empty = emptyPlace();
          if(empty&0x01)
            printf("1\n");
          if(empty&0x02)
            printf("2\n");
          if(empty&0x04)
            printf("3\n");
          if(empty&0x08)
            printf("4\n");
          if(!(empty&0x0f))
            printf("empty\n");
          break;
        case 'T':
          putGoods(4);
          //putDown(2,4);
          
        default:
        printf("default\n");
          break;
      }
    }
    pc = c;
    //wb_robot_cleanup();
  }
  
  return 0;
}
